﻿using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Data;
using System.Drawing;

using System.Text;
using System.Windows.Forms;
using System.Drawing.Drawing2D;

using System.Windows;

namespace AI
{
    public partial class AIInfo
    {
        public double _SegX;
        public double _SegY;

        public double _ZoomX;
        public double _ZoomY;


        Graphics _g;
        Pen _Cur_Pen = new Pen(Color.Blue, 2f);
        Pen _Dest_Pen = new Pen(Color.Black, 2f);

        Pen _Way_Pen = new Pen(Color.Yellow, 2f);

        Pen _Point_Pen = new Pen(Color.White, 2f);

        Pen _Line_Pen = new Pen(Color.White, 1f);

        Pen _KIA_Pen = new Pen(Color.Red, 2f);

        Pen _Cursor = new Pen(Color.Orange, 1f);

        Pen _Signal_Pen = new Pen(Color.Black, 2f);
        //double _CurX = 500 ;
        //double _CurY = 100;
        //double _DestY= 800;
        //double _DestX = 2000;

        //public double _WayX;
        //public double _WayY;

        public bool _bFirstCheck = false;

        public MapInfoManager _MIM = new MapInfoManager();

        bool bFirstCheckDir = true;


        public Point _NextPoint = null;

        public Vector _CursorPoint = new Vector();
        //public Vector _CurPos = new Vector();
        //public Vector _CarDir;
        //public Vector _TargetDir;

        public void AddLog(Object obj)
        {
            listBoxLog.Items.Insert(0, obj);
        }


        public int _SignalCheckCount = 8;

        public int _ObstacleAngle = -1;


        //double _len = 0; 속도 테스트 코드
        //Vector _before;

        //double _vc = 0;

        //Vector nnn = new Vector(x, y);

        //    if (_before.X == 0)
        //    {
        //        _before = nnn;
        //    }



        //    Vector sub = nnn - _before;

        //    sub *= 10000000000.0d;

        //    if (4 < sub.Length)
        //    {
        //        _vc += 1;
        //        _len += sub.Length / _vc;
        //    }


        //    AddLog(x.ToString() + "-" + y.ToString());

        //    AddLog(_len.ToString());

        //    _before = nnn;

        Vector _BeforeMovePoint = new Vector(0, 0);
        Vector _MovePoint ;
        double _MoveRange = 0.0f;

        void NextPoint()
        {
            if( _NextPoint != null)
                _BeforeMovePoint = _NextPoint.GPSPos;
            //_NextPoint = _MIM.GetNextPoint();
            _NextPoint = _MIM.GetNextPoint2();

            if (_NextPoint == null)
            {
                AddLog("return next point");
                return;
            }


            //if (_NextPoint.Signal)
            //{
            //    _NodeSpeed = 0.7d;
            //    _SignalCheckCount = 8;
            //    EventCamUpdateState body = new EventCamUpdateState();
            //    body.bUpdate = true;

            //    //MoveCar(0, 0);

            //    _mainPort.Post(new EventCamUpdate(body));
            //    //캠정보기다림
            //    _Signal.RCount = uint.MaxValue;// 정지후

            
            //}
            //else
            //{
            //    _SignalCheckCount = 0;
            //    _NodeSpeed = 1.0d;

            //    EventCamUpdateState body = new EventCamUpdateState();
            //    body.bUpdate = false;

            //    _mainPort.Post(new EventCamUpdate(body));

            //}

            if (_NextPoint.Range == 0)
            {
                _MoveRange = 0.00005d;
            }
            else
            {
                _MoveRange = 0.00005 * _NextPoint.Range;
            }


            if (_BeforeMovePoint.X != 0)
            {// 처음시작말고 나머지는 경로 살짝 우측을 목적지로 갖는다.
                if (_NextPoint.bShift)
                    //_MovePoint = MathUtil.GetNextPos((float)_BeforeMovePoint.X, (float)_BeforeMovePoint.Y, (float)_NextPoint.GPSPos.X, (float)_NextPoint.GPSPos.Y);
                    _MovePoint = _NextPoint.GPSPos;
                else
                    _MovePoint = _NextPoint.GPSPos;

                
            }
            else
            {
                _MovePoint = _NextPoint.GPSPos;
            }

            

        }

        //double _NextVel = -1;// 초당 진행한 속도


        
        
        
        //double _BeforeMoveDir = 0;// 바로전 이동하려던 위치
        
        //double _BeforLeft = 0;
        //double _BeforRight = 0;



        double _NodeSpeed = 1.0d;


        public void SetTargetPos(double x, double y)
        {
            _DestInfo._EndPoint.X = x;
            _DestInfo._EndPoint.Y = y;
        }

        


        int _LastCount = 0;


        public void SetCurrentPos2(double x, double y, double dx, double dy)
        {

            

            
            if (checkBoxMoveStop.Checked)
                return;

            x = -x;
            dx = -dx;

            _MyCar._Pos.X = x;
            _MyCar._Pos.Y = y;


            AddLog(_MyCar._Pos.ToString());

            //if (_Signal == null)
            //    return;

            //if (0 < _ObstacleAngle)
            //{
            //    AddLog("Obstacle :" + _ObstacleAngle.ToString());
            //    if (60 < _ObstacleAngle && _ObstacleAngle < 90)
            //    {
            //        MoveCar(0.01d, 0.1d);
            //    }
            //    else if (90 < _ObstacleAngle && _ObstacleAngle < 120)
            //    {
            //        MoveCar(0.1d, 0.01d);
            //    }

            //    _ObstacleAngle = -1;
            //    return;
            //}


            ////초반 갱신이 안되였으면
            //if (dx == 0 && dy == 0)
            //{
            //    MoveCar(1.0d, 1.0d);
            //    return;
            //}



            _MyCar._Dir = _MIM._BaseNode.GetDir(dx, dy);

            //_MyCar.Pos = _MyCar.Pos + _MyCar.Dir * _NextVel;


            if (bFirstCheckDir == true)
            {
                //DriveInfo body = new DriveInfo();
                //body.left = 0.5;
                //body.right = 0.5;

                //_mainPort.Post(new NotifyDrive(body));
                bFirstCheckDir = false;


                _DestInfo.StartDir = _MyCar._Dir;
                _DestInfo.StartPoint = _MyCar._Pos;

                _MIM.MakePath( _DestInfo.EndPoint.X, _DestInfo.EndPoint.Y);

                NextPoint();
                //AddLog(_NextPoint);
            }

        }


        //public void SetCurrentPos(double x, double y, double dx, double dy)
        //{

        //    TimeSpan span = DateTime.Now - _UpdateTime;
        //    //AddLog(span.ToString());

            

        //    if (_DestInfo._EndPoint.X == 0)
        //        return;

        //    if (checkBoxMoveStop.Checked)
        //        return;

        //    x = -x;
        //    dx = -dx;

        //    _MyCar._Pos.X = x;
        //    _MyCar._Pos.Y = y;




        //    if (_Signal == null)
        //        return;



        //    if (0 < _ObstacleAngle)
        //    {
        //        AddLog("Obstacle :" + _ObstacleAngle.ToString());
        //        if (60 < _ObstacleAngle && _ObstacleAngle < 90)
        //        {
        //            MoveCar(0.25d, 0.5d);
        //        }
        //        else if (90 < _ObstacleAngle && _ObstacleAngle < 120)
        //        {
        //            MoveCar(0.5d, 0.25d);
        //        }

        //        _ObstacleAngle = -1;
        //        return;
        //    }

        //    if (0 < _SignalCheckCount)
        //    {

        //        AddLog("Check signal");

        //        if (1 < _Signal.RCount)
        //        {
        //            // 처음 시작할때 바로 앞에 신호등을 검색해서 빨간색이면 멈춤
        //            MoveCar(0, 0);
        //            _SignalCheckCount--;
        //            AddLog("Check red");
        //            return;

        //        }

        //    }


        //    //초반 갱신이 안되였으면
        //    if (dx == 0 && dy == 0)
        //    {
        //        MoveCar(0.5d, 0.5d);
        //        return;
        //    }

        //    //_PrePos = this._MyCar.Pos;


        //    //Vector DiffDir = _MyCar._Pos - _PrePos;
        //    //if (DiffDir.Length * 100000 < 5)//조심
        //    //    return;

        //    _MyCar._Dir = _MIM._BaseNode.GetDir(dx, dy);

        //    _MyCar.Pos = _MyCar.Pos + _MyCar.Dir * _NextVel;

        //    //AddLog(_CurPos);

        //    //if (_PrePos.X != 0)
        //    {
        //        if (bFirstCheckDir == true)
        //        {
        //            //DriveInfo body = new DriveInfo();
        //            //body.left = 0.5;
        //            //body.right = 0.5;

        //            //_mainPort.Post(new NotifyDrive(body));
        //            bFirstCheckDir = false;


        //            _DestInfo.StartDir = _MyCar._Dir;
        //            _DestInfo.StartPoint = _MyCar._Pos;

        //            _MIM.MakePath(_DestInfo.StartPoint.X, _DestInfo.StartPoint.Y, _DestInfo.GetNormalDir(), _DestInfo.EndPoint.X, _DestInfo.EndPoint.Y);

        //            NextPoint();
        //            //AddLog(_NextPoint);
        //        }

        //        // 가상으로 돌릴 오브젝트의 속도를 예측할수있도록 한다.
        //        // _MyCar.UpdateDiffInfo(DiffDir.X, DiffDir.Y);



        //        //_MyCar.UpdateMove();

        //         if (Vector.Subtract(_MyCar.Pos, _NextPoint._GPSPos).Length < _NextPoint.Range)
        //         {
        //             if (_NextPoint.Range == 0.00001d)
        //             {
        //                 MoveCar(0.0d, 0.0d);
        //                 return;
        //             }
        //             else
        //             {
        //                 NextPoint();
        //             }
                    
        //         }

        //        if (_NextPoint == null)
        //        {
        //            // 마지막에는 목적지로.
        //            _NextPoint = new Point();
        //            _NextPoint._GPSPos = _DestInfo.EndPoint;
        //            _NextPoint.Range = 0.00001d;
        //            return;
        //        }
        //        else
        //        {
        //        }


        //        //if (0 < _SignalCheckCount)
        //        //{
        //        //    MoveCar(0, 0);

        //        //    return;
        //        //}


        //        //비전으로 위점지역으로 간다고 판단되면
        //        // 128*128/2/2 * 2/3 74
        //        //128*54/2/3 = 1152
        //        // 위험지역 2/3가 넘어가면 

        //        int Threshold = 1152;

        //        if (Threshold * 2 < _Signal.LeftCount && _Signal.RightCount < Threshold * 2 )
        //        {

        //            MoveCar(0.5d, 0.35d);

        //            AddLog("move force right");

        //            return;
        //        }
        //        else if (Threshold * 2 < _Signal.RightCount && _Signal.LeftCount <  Threshold * 2 )
        //        {

        //            MoveCar(0.35d, 0.5d);
        //            AddLog("move force left");
        //            return;
        //        }

        //        //Vector NextPos = _MyCar.Pos + _MyCar.Dir * _NextVel;

        //        Vector _TargetDir = _NextPoint._GPSPos - _MyCar.Pos;

        //        _TargetDir = _MIM._BaseNode.GetDir(_TargetDir.X, _TargetDir.Y);

        //        //double velocity = Vector.Multiply(_TargetDir, _MyCar._Dir) / 4 + 0.25;

        //        double rad = Vector.Multiply(_MyCar._Dir, _TargetDir);
        //        double angle = Math.Acos(rad) * 180 / Math.PI;

        //        double vel = 0.5;
        //        double speed = 0.5;

        //        if (angle < 90)
        //        {
        //            vel = 1.0d - (0.7d * angle / 90.0d);
        //            speed = 1.0 - (0.5 * angle / 90);
        //        }

        //        float carDegree = MathUtil.GetDegreesForBaseRotation((float)_MyCar._Dir.X, (float)_MyCar._Dir.Y);
        //        float nextTargetDegree = MathUtil.GetDegreesForBaseRotation((float)_TargetDir.X, (float)_TargetDir.Y);

        //        int bRight;

        //        if (carDegree < nextTargetDegree)
        //            bRight = 1;
        //        else
        //            bRight = -1;

        //        if (180 < Math.Abs(carDegree - nextTargetDegree))
        //        {
        //            bRight *= -1;

        //        }

        //        if (bRight == -1)
        //        {
        //            double left = (double)1 * speed * _NodeSpeed;
        //            double right = vel * speed * _NodeSpeed; ;

        //            MoveCar(left, right);
        //        }
        //        else
        //        {
        //            double left = vel * speed * _NodeSpeed;
        //            double right = (double)1 * speed * _NodeSpeed;
        //            MoveCar(left, right);
        //        }


        //    }


        //}

        public void SetWayPos(double x, double y)
        {
            //_WayX = x;
            //_WayY = y;

        }

        //Vector _PrePos;


        //public void SetDestPos(UrbanDestInfoState DestInfo)
        //{

        //    //_DestX = x;
        //    //_DestY = y;

        //    //Dest_Longitude_X.Text = _DestX.ToString();
        //    //Dest_Latitude_Y.Text = _DestY.ToString();
        //}

        public void InitMap(Graphics g)
        {
            _g = g;

            Matrix m = new Matrix(-1, 0, 0, -1, 0, 0);


            m.Translate(1200f, 700f, MatrixOrder.Append);

            _g.Transform = m;


            //if (_CurX < _DestX)
            //    _SegX = _CurX;
            //else _SegX = _DestX;

            //if (_CurY < _DestY)
            //    _SegY = _CurY;
            //else _SegY = _DestY;

            //_ZoomX = Math.Abs(_CurX - _DestX) / 500;
            //_ZoomY = Math.Abs(_CurY - _DestY) / 500;

            //_ZoomX += 0.1;
            //_ZoomY += 0.1;


        }

        public void DrawLine(Pen pen, Vector sp, Vector ep)
        {
            sp = sp * 100000;
            ep = ep * 100000;

            _g.DrawLine(pen, (int)(sp.X - 12232000) * 2, (int)(sp.Y - 4760000) * 2, (int)(ep.X - 12232000) * 2, (int)(ep.Y - 4760000) * 2);
        }

        public void DrawEllipse(Pen pen, double sx, double sy, int size)
        {
            sx = sx * 100000 - 12232000;
            sy = sy * 100000 - 4760000;
            sx *= 2;
            sy *= 2;

            _g.DrawEllipse(pen, (float)sx - size / 2, (float)sy - size / 2, size, size);

            //_g.DrawEllipse(pen, (float)sx - size/2, (float)sy - size/2, size, size);
        }

        public void Draw()
        {

            if (checkGPSPos.Checked)
                return;
            //{

            //    foreach (Node node in _MIM._BaseNode._NodeList.Values)
            //    {
            //        DrawLine(_Line_Pen,_MIM._BaseNode._PointList[node._SIndex]._GPSPos, _MIM._BaseNode._PointList[node._EIndex]._GPSPos);

            //    }

            //    return;
            //}


            //double cx = (_CurX - _SegX) / _ZoomX;
            //double cy = (_CurY - _SegY) / _ZoomY;

            //double dx = (_DestX - _SegX) / _ZoomX;
            //double dy = (_DestY - _SegY) / _ZoomY;

            //_g.DrawEllipse(_Cur_Pen, (float)cx-3, (float)cy-3, 6, 6);
            //_g.DrawEllipse(_Cur_Pen, (float)dx-3, (float)dy-3, 6, 6);

            foreach (Point p in _MIM._BaseNode._PointList.Values)
            {


                if (p.Signal)
                {
                    DrawEllipse(_Signal_Pen, p.GPSPos.X, p.GPSPos.Y, 10);
                }
                else
                {
                    DrawEllipse(_Point_Pen, p._GPSPos.X, p._GPSPos.Y, 6);
                }

            }


            foreach (Node node in _MIM._BaseNode._NodeList.Values)
            {
                DrawLine(_Line_Pen, _MIM._BaseNode._PointList[node._SIndex]._GPSPos, _MIM._BaseNode._PointList[node._EIndex]._GPSPos);

            }


            if (_MIM._PathInfo != null && 2 < _MIM._PathInfo._Path.Count)
            {
                //System.Drawing.Point[] way = new System.Drawing.Point[this._MIM._BestNodePath._TotalNodeList.Count];


                Point sp = _MIM._BaseNode._PointList[_MIM._PathInfo._Path[0]];

                for (int index = 1; index < _MIM._PathInfo._Path.Count; index++)
                {
                    Point ep = _MIM._BaseNode._PointList[_MIM._PathInfo._Path[index]];
                    DrawLine(_Way_Pen, sp.GPSPos, ep.GPSPos);
                    sp = ep;
                }



            }
            //if (this._MIM._BestNodePath != null)
            //{
            //    System.Drawing.Point[] way = new System.Drawing.Point[this._MIM._BestNodePath._TotalNodeList.Count];


            //    foreach (int nodeIndex in this._MIM._BestNodePath._TotalNodeList)
            //    {
            //        Node node = _MIM._BaseNode._NodeList[nodeIndex];
            //        Vector sp = _MIM._BaseNode._PointList[node._SIndex]._GPSPos;

            //        Vector ep = _MIM._BaseNode._PointList[node._EIndex]._GPSPos;
            //        DrawLine(_Way_Pen, sp, ep);

            //    }

            //}

            foreach (int index in _MIM._KiaPoint)
            {
                Point p = _MIM._BaseNode._PointList[index];
                DrawEllipse(_KIA_Pen, p._GPSPos.X, p._GPSPos.Y, 6);
            }

            _CurMode.Draw(_g);

            DrawEllipse(_Cursor, _CursorPoint.X, _CursorPoint.Y, 6);

        }
    }
}
